#!/usr/bin/env python

import rospy
from sensor_msgs.msg import Image
from cv_bridge import CvBridge
from apriltags2_ros.msg import AprilTagDetectionArray,AprilTagDetection

import cv2
import numpy as np


points = []



def image_callback(data):
    bridge = CvBridge()
    cv_image = bridge.imgmsg_to_cv2(data, "bgr8")

    height, width, channels = cv_image.shape
    
    print("image size:"+str(height)+"*"+str(width))
    

    

    if len(points) == 4:
    #print(points)
        cv2.fillPoly(cv_image, np.array([points], dtype=np.int32), (0, 0, 0))
        
        for i in range(4):
             pt1 = points[i]
             pt2 = points[(i + 1) % len(points)]

             dist = np.sqrt((pt2[0] - pt1[0])**2 + (pt2[1] - pt1[1])**2)
             direction = ((pt2[0] - pt1[0]) / dist, (pt2[1] - pt1[1]) / dist)
        

             for j in range(int(dist) + 1):
                x = int(pt1[0] + j * direction[0])
                y = int(pt1[1] + j * direction[1])
                cv2.circle(cv_image, (x, y), 13, (0, 0, 0), -1)
            

    img_with_polygon_msg = bridge.cv2_to_imgmsg(cv_image, "bgr8")
    pub.publish(img_with_polygon_msg)

def points_callback(data):
    global points
    points=[]
    detection_array=data
    
    #det=detection_array.detections[0]
    
    for i in range(4):
        corner_point=det.polygon.points[i]
        points.append((int(corner_point.x),int(corner_point.y)))
    
        
    #print(points)
        
def polygon_drawer():
    global pub
    rospy.init_node('polygon_drawer', anonymous=True)
    pub = rospy.Publisher('/image_with_polygon', Image, queue_size=10)
    rospy.Subscriber('/tag_detections_image', Image, image_callback)
    #rospy.Subscriber('/tag_detections', AprilTagDetectionArray, points_callback)
    rospy.spin()

if __name__ == '__main__':
    polygon_drawer()